#pragma once

#include <XnCppWrapper.h>
#include <opencv2\core\core.hpp>
#include <opencv2\highgui\highgui.hpp>
#include <aruco\aruco.h>
#include <Aria.h>
#include <string>
#include <vector>
#include <sstream>
#include "MyMarker.h"
#include "ArLog.h"
#include "RobotUtils.h"
#include "KinectMotor.h"

using namespace cv;


class VisualData
{
	
	VideoWriter vw;
	aruco::MarkerDetector detector;
	cv::VideoCapture capture;
	///	posizione e orientazione iniziali della telecamera rispetto al sistema globale
	ArPose initialPose;

	///	angolo di rotazione della telecamera rispetto al suo asse x
	///	(solitamente negativo, ovvero verso il basso)
	int cameraRotAngle;
	double cameraRotAngleRad;

	///	matrice di rotazione dal sistema camera 3d al sistema camera 3d ruotato
	cv::Mat_<double> cameraRot;

	int tiltAngle;

	///	tutti i marker presenti nell'ambiente di movimento
	std::vector<MyMarker> globalMarkers;

	///	marker rilevati da aruco
	std::vector<aruco::Marker> detectedMarkers;

	///	contatore globale dei marker trovati da questa telecamera
	int markersFound;

	///	prefisso per i file immagine dei marker trovati
	string markerFoundFileName;

	///	
	Vec3f getRelativeCoor(int x, int y, cv::Mat depth);

	///	controlla che il punto trovato dalla telecamera sia:
	///	1)	a profondita' positiva (non rilevo punti che siano dietro)
	///	2)	a distanza superiore rispetto alla distanza minima
	///			(per non considerare rilevamenti erronei)
	bool check3DPoint(Vec3f &point) const;

public:
	///	distanza minima dalla telecamera,
	///	sotto la quale considero non attendibili i punti rilevati
	static const unsigned short NEAREST_DISTANCE = 100;
	int framecounter;
	VisualData(void);
	~VisualData(void);

	/**	Cerca i marker attualmente inquadrati dalla telecamera
	*	
	*	@param markers i marker trovati sono messi qui
	*/
	bool findMarkers(std::vector<MyMarker> &markers);

	/**	Calcola la posizione della telecamera rispetto al sistema globale
	*	date le posizioni di 2 punti rispetto ai sistemi camera 2d e a quello globale
	*	
	*	@param P_INFc2d	punto inferiore rispetto al sistema camera 2d
	*	@param P_SUPc2d	punto superiore rispetto al sistema camera 2d
	*	@param P_INF	punto inferiore rispetto al sistema globale
	*	@param P_SUP	punto superiore rispetto al sistema globale
	*/
	ArPose getPoseFromMarker(const cv::Mat_<double> &P_INFc2d,
		const cv::Mat_<double> &P_SUPc2d,
		const cv::Mat_<double> &P_INF,
		const cv::Mat_<double> &P_SUP) const;

	/**	Calcola la posizione della telecamera rispetto al sistema globale,
	*	rilevando i marker e conoscendo la loro posizione globale
	*	
	*	@param globalPose la posizione corretta coi dati visuali e' messa qui
	*/
	bool getGlobalPose(ArPose &globalPose);



};
